/*
 * @Author: junhuizhou
 * @Date: 2020-12-30 10:58:59
 * @LastEditor: junhuizhou
 * @LastEditTime: 2020-12-30 15:35:42
 * @Description: header
 * @FilePath: \fzhtq\src\caltransmat.c
 */

#include <stdio.h>
#include <math.h>
#include "../include/caltransmat.h"
#include "../include/calmatrix.h"
#include "../include/constant.h"

void Mat2Quat(double matatt[3][3], double quat[4])
{
    double tmpvec[4];
    int i=0, maxid=0;
    double tmpmax = 0.0;
    tmpvec[0] = 1 + matatt[0][0] + matatt[1][1] + matatt[2][2];
    tmpvec[1] = 1 + matatt[0][0] - matatt[1][1] - matatt[2][2];
    tmpvec[2] = 1 - matatt[0][0] + matatt[1][1] - matatt[2][2];
    tmpvec[3] = 1 - matatt[0][0] - matatt[1][1] + matatt[2][2];
    for(i=0; i<4; i++)
    {
        if(tmpvec[i] > tmpmax)
        {
            tmpmax = tmpvec[i];
            maxid = i;
        }
    }

    quat[maxid] = sqrt(tmpvec[maxid])/2.0;
    switch(maxid)
    {
        case 0:
            quat[1] = (matatt[2][1] - matatt[1][2]) / 4 / quat[0];  /*q1*/
            quat[2] = (matatt[0][2] - matatt[2][0]) / 4 / quat[0];  /*q2*/
            quat[3] = (matatt[1][0] - matatt[0][1]) / 4 / quat[0];  /*q3*/
            break;
        case 1:
            quat[0] = (matatt[2][1] - matatt[1][2]) / 4 / quat[1];
            quat[2] = (matatt[1][0] + matatt[0][1]) / 4 / quat[1];
            quat[3] = (matatt[0][2] + matatt[2][0]) / 4 / quat[1];
            if(quat[0] < 0)
            {
                quat[0] = -quat[0];
                quat[1] = -quat[1];
                quat[2] = -quat[2];
                quat[3] = -quat[3];
            }
            break;
        case 2:
            quat[0] = (matatt[0][2] - matatt[2][0]) / 4 / quat[2];
            quat[1] = (matatt[1][0] + matatt[0][1]) / 4 / quat[2];
            quat[3] = (matatt[2][1] + matatt[1][2]) / 4 / quat[2];
            if(quat[0] < 0)
            {
                quat[0] = -quat[0];
                quat[1] = -quat[1];
                quat[2] = -quat[2];
                quat[3] = -quat[3];
            }
            break;
        case 3:
            quat[0] = (matatt[1][0] - matatt[0][1]) / 4 / quat[3];
            quat[1] = (matatt[0][2] + matatt[2][0]) / 4 / quat[3];
            quat[2] = (matatt[2][1] + matatt[1][2]) / 4 / quat[3];
            if(quat[0] < 0)
            {
                quat[0] = -quat[0];
                quat[1] = -quat[1];
                quat[2] = -quat[2];
                quat[3] = -quat[3];
            }
            break;
    }
}

void Mat2QuatRot(double qrot[4][4], double matatt[3][3])
{
    double quat[4];
    Mat2Quat(matatt, quat);
    Quat2Qrot(quat, qrot);
}

void Quat2Mat(double quat[4], double matatt[3][3])
{
    matatt[0][0] = 1 - 2*quat[2]*quat[2] - 2*quat[3]*quat[3];
    matatt[0][1] = 2 * (quat[1]*quat[2] - quat[3]*quat[0]);
    matatt[0][2] = 2 * (quat[1]*quat[3] + quat[2]*quat[0]);
    matatt[1][0] = 2 * (quat[1]*quat[2] + quat[3]*quat[0]);
    matatt[1][1] = 1 - 2*quat[1]*quat[1] - 2*quat[3]*quat[3];
    matatt[1][2] = 2 * (quat[2]*quat[3] - quat[1]*quat[0]);
    matatt[2][0] = 2 * (quat[1]*quat[3] - quat[2]*quat[0]);
    matatt[2][1] = 2 * (quat[2]*quat[3] + quat[1]*quat[0]);
    matatt[2][2] = 1 - 2*quat[1]*quat[1] - 2*quat[2]*quat[2];
}

void Quat2Qrot(double quat[4], double qrot[4][4])
{
    qrot[0][0] = quat[0];
    qrot[0][1] = -quat[1];
    qrot[0][2] = -quat[2];
    qrot[0][3] = -quat[3];
    qrot[1][0] = quat[1];
    qrot[1][1] = quat[0];
    qrot[1][2] = -quat[3];
    qrot[1][3] = quat[2];
    qrot[2][0] = quat[2];
    qrot[2][1] = quat[3];
    qrot[2][2] = quat[0];
    qrot[2][3] = -quat[1];
    qrot[3][0] = quat[3];
    qrot[3][1] = -quat[2];
    qrot[3][2] = quat[1];
    qrot[3][3] = quat[0];
}

void Qrot2Mat(double qrot[4][4], double matatt[3][3])
{
    double quat[4];
    int i = 0;
    for(i=0; i<4; i++)
    {
        quat[i] = qrot[i][0];
    }
    Quat2Mat(quat, matatt);
}

void Quat2Euler(double quat[4], double euler[3])
{
    double epsilon = 0.0009765625f;
    double threshold = 0.5f - epsilon;
    double odd = quat[0]*quat[2] - quat[1]*quat[3];
    
    if(odd<threshold && odd>-threshold)
    {
        euler[0] = atan2(2*(quat[0]*quat[1] + quat[2]*quat[3]), 1 - 2*(quat[1]*quat[1]+quat[2]*quat[2]));
        euler[1] = asin(2*(quat[0]*quat[2] - quat[1]*quat[3]));
        euler[2] = atan2(2*(quat[0]*quat[3] + quat[1]*quat[2]), 1 - 2*(quat[2]*quat[2]+quat[3]*quat[3]));
    }
    else    /*处理奇异值*/
    {
        if(odd > 0)
        {
            euler[0] = 0;
            euler[1] = PI/2.0;
            euler[2] = -2*atan2(quat[1], quat[0]);
        }
        else
        {
            euler[0] = 0;
            euler[1] = -PI/2.0;
            euler[2] = 2*atan2(quat[1], quat[0]);
        }
    }
}

void Euler2Quat(double euler[3], double quat[4])
{
    double cx = cos(euler[0]*0.5);
    double sx = sin(euler[0]*0.5);
    double cy = cos(euler[1]*0.5);
    double sy = sin(euler[1]*0.5);
    double cz = cos(euler[2]*0.5);
    double sz = sin(euler[2]*0.5);
    quat[0] = cx*cy*cz + sx*sy*sz;
    quat[1] = sx*cy*cz - cx*sy*sz;
    quat[2] = cx*sy*cz + sx*cy*sz;
    quat[3] = cx*cy*sz - sx*sy*cz;
}

void MatRotX(double matRotX[3][3], double angleRoll)
{
    double tmpsin, tmpcos;
    tmpsin = sin(angleRoll);
    tmpcos = cos(angleRoll);
    matRotX[0][0] = 1;
    matRotX[0][1] = 0;
    matRotX[0][2] = 0;
    matRotX[1][0] = 0;
    matRotX[1][1] = tmpcos;
    matRotX[1][2] = -tmpsin;
    matRotX[2][0] = 0;
    matRotX[2][1] = tmpsin;
    matRotX[2][2] = tmpcos;
}

void MatRotY(double matRotY[3][3], double anglePitch)
{
    double tmpsin, tmpcos;
    tmpsin = sin(anglePitch);
    tmpcos = cos(anglePitch);
    matRotY[0][0] = tmpcos;
    matRotY[0][1] = 0;
    matRotY[0][2] = tmpsin;
    matRotY[1][0] = 0;
    matRotY[1][1] = 1;
    matRotY[1][2] = 0;
    matRotY[2][0] = -tmpsin;
    matRotY[2][1] = 0;
    matRotY[2][2] = tmpcos;
}

void MatRotZ(double matRotZ[3][3], double angleYaw)
{
    double tmpsin, tmpcos;
    tmpsin = sin(angleYaw);
    tmpcos = cos(angleYaw);
    matRotZ[0][0] = tmpcos;
    matRotZ[0][1] = -tmpsin;
    matRotZ[0][2] = 0;
    matRotZ[1][0] = tmpsin;
    matRotZ[1][1] = tmpcos;
    matRotZ[1][2] = 0;
    matRotZ[2][0] = 0;
    matRotZ[2][1] = 0;
    matRotZ[2][2] = 1;
}
